// feature extraction for object identifier

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <vector>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "ObjectIdentifier.h"

using namespace std;
using namespace cv;

#define IS_BLACK   (hsv[2] < 50)
#define IS_BLUE    (hsv[0] > 85 && hsv[0] < 115)
#define IS_GREEN   (hsv[0] > 35 && hsv[0] < 65)
#define IS_MAZENDA (hsv[0] > 135)

namespace marker_detector
{


int ObjectIdentifier::extract_color(cv::Mat &bgr_img, cv::Mat &mono_img)
{
  // check most frequent color (except white)
  cv::Mat hsv_img(bgr_img.size(), CV_8UC3);
  cvtColor(bgr_img, hsv_img, CV_BGR2HSV);

  int color = _get_color(hsv_img);
  if(color < 0) return color;
  ROS_DEBUG("Symbol color: %d", color);

  // binarize
  vector<cv::Mat> hsv(3);
  split(hsv_img, hsv);
  Mat mask_img;
  switch(color)
  {
    case K:
      threshold(hsv[2], mono_img, 100, 255, CV_THRESH_BINARY);
      mono_img = ~mono_img;
      break;
    case B:
      threshold(hsv[0], mono_img, 100 + 15, 255, CV_THRESH_TOZERO_INV);
      threshold(mono_img, mono_img, 100 - 15, 255, CV_THRESH_BINARY);
      break;
    case G:
      // green and white are similare in Hue space.
      // we may better conduct masking process before
      //threshold(hsv[2], mask_img, 100, 255, CV_THRESH_BINARY_INV);
      //hsv[0].copyTo(mono_img, mask_img);
      //threshold(mono_img, mono_img, 50 + 15, 255, CV_THRESH_TOZERO_INV);
      threshold(hsv[0], mono_img, 55 + 15, 255, CV_THRESH_TOZERO_INV);
      threshold(mono_img, mono_img, 55 - 15, 255, CV_THRESH_BINARY);
      break;
    case M:
      threshold(hsv[0], mono_img, 135, 255, CV_THRESH_BINARY);
      break;
  }

  return color;
}

//! pick 16 non-white points randomly
//! then, vote for the closet color
int ObjectIdentifier::_get_color(cv::Mat &hsv_img)
{
  int N = 16;
  srand(time(NULL));

  // voting initialize
  vector<int> vote(4);
  fill(vote.begin(), vote.end(), 0);

  FOR(i, 0, N)
  {
    if(N > 1000) break;
    int x = rand() % hsv_img.cols;
    int y = rand() % hsv_img.rows;
    cv::Vec3b hsv = hsv_img.ptr<cv::Vec3b>(y)[x];
    //ROS_INFO("Hue: %d, V: %d", hsv[0], hsv[2]);
    bool voted = false;
    if(IS_BLUE) { vote[B]++; voted = true; }
    if(IS_GREEN) { vote[G]++; voted = true; }
    if(IS_MAZENDA) { vote[M]++; voted = true; }
    if(IS_BLACK) { vote[K]++; voted = true; }
    if(!voted) N++; // white
  }
  //FOR(i, 0, 4) ROS_ERROR("%d (%d) --> %d", i, N, vote[i]);

  // find max
  int color = max_element(vote.begin(), vote.end()) - vote.begin();
  return (vote[color] > 0)? color: -1;
}

//! extract bag of features from img
//! output will be stored in f (vector<float>)
void ObjectIdentifier::extract_feature(cv::Mat &img, Feature_t &f)
{
  int kernel_size = 7;
  double sig = 3;
  double lm = 1;
  double th = 0;
  double ps = 90;
  int histsize = 16;
  vector<float> hist;
  cv::Mat hist_img = Mat::zeros(Size(histsize*10, 200), CV_8UC1);
  for(int j = 0; j < 2; j++) {
    sig = 2 + 7*j;
  for(int i = 0; i < 8; i++) {
    th = 22.5 * i;
    Mat kernel = _mk_kernel(kernel_size, sig, th, lm, ps);
    Mat dst;
    filter2D(img, dst, CV_32F, kernel);
    threshold(dst, dst, 100, 255, THRESH_BINARY);
    //imshow("dst", dst);
    //waitKey(100);
    Moments m = moments(dst, true);
    hist.push_back(m.m00);
  }
  }
  double max_val = *max_element(hist.begin(), hist.end());
  if(max_val <= 0.0) max_val = 1.0;
  for(int j = 0; j < 2; j++) {
    for(int i = 0; i < 8; i++) {
      rectangle(hist_img, Point(j*80+i*10, 0), Point(j*80+(i+1)*10, hist.at(8*j+i)/max_val*200), 255, -1);
    }
  }
  //cout << endl;
  flip(hist_img, hist_img, 0);
  imshow("hist", hist_img);
  waitKey(1);
  
  for(int i = 0; i < (int)hist.size(); i++) {
    f.push_back(hist[i]/max_val);
  }
}

//! make kernel for gabor filter
cv::Mat ObjectIdentifier::_mk_kernel(int ks, double sig, double th, double lm, double ps) 
{
  int hks = (ks-1)/2;
  double theta = th*CV_PI/180; // direction
  double psi = ps*CV_PI/180; // phase offset
  double del = 2.0/(ks-1);
  double lmbd = lm;
  double sigma = sig/ks;
  double x_theta;
  double y_theta;
  Mat kernel(Size(ks, ks), CV_32F);
  for(int y = -hks; y <= hks; y++) {
    for(int x = -hks; x <= hks; x++) {
      x_theta = x*del*cos(theta)+y*del*sin(theta);
      y_theta = -x*del*sin(theta)+y*del*cos(theta);
      kernel.at<float>(hks+y, hks+x) =
          (float)exp(-0.5*(pow(x_theta,2)+pow(y_theta,2))/pow(sigma,2))* cos(2*CV_PI*x_theta/lmbd + psi);
    }
  }
  return kernel;
}


} // end of namespace 
